#include "PhysicsObject.h"

#include "Utility.h"
#include "Resources.h"
#include "InputHandler.h"
#include <list>
#include "EntityManager.h"

PhysicsObject::PhysicsObject(sf::RenderWindow *App) : BaseEntity(App)
{
	//ALWAYS PUT THIS FIRST!!!!!!!
	if (App == NULL)
	{
		Register();
		return;
	}

	pRender = App;

	PointCount = 0;
	MOI = 1;
	Mass = 50;

	Solid = true; //Enable collisions
	Moveable = true;

	Bounce = 0.5f;

	Velocity = sf::Vector2f(0,0);
	Acceleration = sf::Vector2f(0,0);
	AngularVelocity = 0;
	AngularAcceleration = 0;
	LinearDamping = 0.05f;
	AngularDamping = 0.05f;

	Resultant.x = 0;
	Resultant.y = 0;
}

PhysicsObject::~PhysicsObject(void)
{
}

void PhysicsObject::PhysicsInitPolygon(BaseEntity* Caller, sf::Vector2f *Points, int PointCount)
{
	for (int i=0; i < PointCount; i++)
	{
		AddPoint(Points[i]);
	}
	Activate(Caller);
}

void PhysicsObject::PhysicsInitCircle(BaseEntity* Caller, float radius)
{

}


void PhysicsObject::Activate(BaseEntity* Creator)
{
	mOwner = Creator;
	CentreG = sf::Vector2f(0,0);
	for (int i = 0; i < PointCount; i++)
	{
		CentreG = CentreG + Coords[i];
	}
	CentreG.x /=  PointCount;
	CentreG.y /= PointCount;

	UpdateMOI();
	//std::cout << "Centre of Gravity: " << CentreG.x << " " << CentreG.y << "\n";
	SetOrigin(CentreG);
	Point = sf::Vector2f(CentreG);
}

void PhysicsObject::UpdateMOI()
{
	MOI = (GetMass()*(std::pow(GetAABB().Width / 2.f, 2) + std::pow(GetAABB().Height / 2.f, 2))) / 24.f;
}

sf::FloatRect PhysicsObject::GetAABB()
{
	sf::Vector2f TL, TR, BL, BR;
	TL = TR = BL = BR = ToGlobal(Coords[0]);
	for (int i=0; i < PointCount; i++)
	{
		sf::Vector2f point = ToGlobal(Coords[i]);

		TL.x = std::min(point.x, TL.x);
		TL.y = std::max(point.y, TL.y);

		TR.x = std::max(point.x, TR.x);
		TR.y = std::max(point.y, TR.y);

		BL.x = std::min(point.x, BL.x);
		BL.y = std::min(point.y, BL.y);

		BR.x = std::max(point.x, BR.x);
		BR.y = std::min(point.y, BR.y);
	}
	TL.x -= 1;
	TL.y -= 1;
	return sf::FloatRect(TL, sf::Vector2f(BR.x - TL.x + 1, TL.y - BR.y + 1));
}

sf::Vector2f PhysicsObject::GetPointVelocity(sf::Vector2f GlobalPoint)
{
	sf::Vector2f Vel;
	sf::Vector2f Offset = ToGlobal(CentreG) - GlobalPoint;
	float speed = Util::DegToRad(GetAngularVel()) * Util::Distance(ToGlobal(CentreG), GlobalPoint);
	Vel = Util::VecNormal(Offset);
	Vel.x = Vel.x * speed;
	Vel.y = Vel.y * speed;

	Vel += GetVelocity();

	return Vel;
}

void PhysicsObject::OnCollide(PhysicsObject* ent1, PhysicsObject* ent2, CollisionData CollideInfo)
{
	float e = 0.f;//std::max(ent1->GetBounce(), ent2->GetBounce());

	float BounceBack = Util::VecDot(Util::VecScale(CollideInfo.RelativeVelocity, -(1 + e)), CollideInfo.Normal);

	float M1Recip = 1/ent1->GetMass();
	float M2Recip = 1/ent2->GetMass();

	if (!ent1->GetMoveable())
		M1Recip = 0.f;
	if (!ent2->GetMoveable())
		M2Recip = 0.f;

	float MassScale = M1Recip + M2Recip;
	MassScale = Util::VecDot(CollideInfo.Normal, Util::VecScale(CollideInfo.Normal, MassScale));

	float P1Factor = Util::VecDot(-Util::VecNormal(ent1->ToGlobal(ent1->CentreG) - CollideInfo.CollisionPoint), CollideInfo.Normal);
	P1Factor = std::pow(P1Factor, 2);

	float P2Factor = Util::VecDot(Util::VecNormal(ent2->ToGlobal(ent2->CentreG) - CollideInfo.CollisionPoint), CollideInfo.Normal);
	P2Factor = std::pow(P2Factor, 2);
	
	float Impulse = BounceBack;
	Impulse /= (MassScale) + (P1Factor/ent1->MOI) + (P2Factor/ent2->MOI);
	
#if DEBUG_PHYSICS == 1
	sf::Vector2f CP = GameCoordsToSFML(CollideInfo.CollisionPoint);
	shpCollPoint=sf::Shape::Rectangle(CP.x - 4, CP.y - 4, 8, 8, sf::Color::Green, 1.f, sf::Color::Black);

	sf::Vector2f LineEnd = GameCoordsToSFML(CollideInfo.CollisionPoint + Util::VecScale(CollideInfo.Normal, 50));
	shpNormal = sf::Shape::Line(GameCoordsToSFML(CollideInfo.CollisionPoint), LineEnd, 2.f, sf::Color::Black, 0.f, sf::Color::Black);
#endif

	if (ent1->GetMoveable())
		AddVelocity(Util::VecScale(CollideInfo.Normal, Impulse / GetMass()));
	if (ent2->GetMoveable())
		ent2->AddVelocity(Util::VecScale(-CollideInfo.Normal, Impulse / ent2->GetMass()));

	sf::Vector2f FricNorm = Util::VecNormal(CollideInfo.Normal);

	sf::Vector2f APPerp1 = Util::VecNormal(ToGlobal(CentreG) - CollideInfo.CollisionPoint);
	sf::Vector2f APPerp2 = Util::VecNormal(ent2->ToGlobal(ent2->CentreG) - CollideInfo.CollisionPoint);

	float AngChange1 = Util::VecDot(APPerp1, Util::VecScale(CollideInfo.Normal ,Impulse));
	AngChange1 /= ent1->MOI;
	float AngChange2 = Util::VecDot(APPerp2, Util::VecScale(CollideInfo.Normal ,Impulse));
	AngChange2 /= ent2->MOI;

	if (ent1->GetMoveable())
		ent1->SetAngularVel(ent1->GetAngularVel() + Util::RadToDeg(AngChange1));
	if (ent2->GetMoveable())
		ent2->SetAngularVel(ent2->GetAngularVel() - Util::RadToDeg(AngChange2));
}

sf::Vector2f PhysicsObject::FarthestPoint(sf::Vector2f dir)
{
	int FarID = -1;
	float FarMag = -1;
	for (int i=0; i < GetPointCount(); i++)
	{
		float dot = Util::VecDot(ToGlobal(Coords[i]), dir);
		if (dot > FarMag)
		{
			FarID = i;
			FarMag = dot;
		}
	}
	return ToGlobal(Coords[FarID]);
}	


void PhysicsObject::ApplyForceOffset(sf::Vector2f Force, sf::Vector2f Offset)
{
	Resultant += Force;
	ResultantPos = Util::VecScale(Offset + ResultantPos, 0.5);
}

void PhysicsObject::AddPoint(sf::Vector2f Point)
{
	Coords.push_back(Point);
	PointCount++;
}

void PhysicsObject::Integrate(float delta)
{
	SetPos(GetOwner()->GetPos());
	SetAngle(GetOwner()->GetAngle());
	if (GetMoveable())
	{
		sf::Vector2f Offset = ResultantPos;

		Offset.x /= Mass;
		Offset.y /= Mass;

		float Torque = Offset.y * Resultant.x;
		Torque += Offset.x * -Resultant.y;

		AngularAcceleration = Torque / 24.f;
		Acceleration = Util::VecScale(Resultant, 1/Mass);

		SetAngularVel(GetAngularVel() + AngularAcceleration);
		AddVelocity(Acceleration);

		SetPos(GetPos() + (GetVelocity() * delta) + (Util::VecScale(Acceleration, 0.5f * delta * delta)));
		SetAngle(Angle + (AngularVelocity * delta) + (AngularAcceleration * 0.5f * delta * delta));

		AddVelocity(GetVelocity() * -LinearDamping);
		SetAngularVel(GetAngularVel() + (GetAngularVel() * -AngularDamping));

	}
	GetOwner()->SetPos(GetPos());
	GetOwner()->SetAngle(GetAngle());
	AABB = GetAABB();
}

void PhysicsObject::Think()
{ 

#if DEBUG_PHYSICS == 1
	sf::Vector2f AABBOrigin = GameCoordsToSFML(sf::Vector2f(AABB.Left,AABB.Top));
	shpAABB = sf::Shape::Rectangle(AABBOrigin.x, AABBOrigin.y, AABB.Width, AABB.Height, sf::Color(255,255,255,0), 1.f, sf::Color::Black);

	sf::Vector2f LineEnd = GameCoordsToSFML(ToGlobal(Coords[0]) + GetPointVelocity(ToGlobal(Coords[0])));
#endif

	Resultant.x = 0.f;
	Resultant.y = 0.f;
	ResultantPos.x = 0.f;
	ResultantPos.y = 0.f;

	NextThink(0.f);
}

void PhysicsObject::Draw()
{
#if DEBUG_PHYSICS == 1
	pRender->Draw(shpAABB);
	pRender->Draw(shpCollPoint);
	pRender->Draw(shpNormal);

	for (unsigned int Points2=0; Points2 < PointCount; Points2++)
	{
		unsigned int NextPoint = Points2 + 1;
		if (NextPoint == GetPointCount())
			NextPoint = 0;
		sf::Vector2f Line1 = GameCoordsToSFML(ToGlobal((Coords[Points2])));
		sf::Vector2f Line2 = GameCoordsToSFML(ToGlobal(Coords[NextPoint]));

		sf::Vector2f TestPoint = GameCoordsToSFML(ToGlobal(Coords[Points2]));

		sf::Vector2f Pvel = GetPointVelocity(Util::VecScale(ToGlobal(Coords[Points2]) + ToGlobal(Coords[NextPoint]), 0.5));
		Pvel.y *= -1;
		Pvel = Util::VecScale(Pvel, 0.5);
		sf::Vector2f LineEnd = Util::VecScale(Line1 + Line2, 0.5) + Pvel;
		switch(Points2)
		{
		case 0:
			sL1 = sf::Shape::Line(Line1, Line2, 1.f, sf::Color::Red, 0.f, sf::Color::Red);
			pRender->Draw(sL1);
			sP1 = sf::Shape::Rectangle(TestPoint.x, TestPoint.y, 4, 4, sf::Color::Green, 1.f, sf::Color::Green);  
			pRender->Draw(sP1);
			sN1 = sf::Shape::Line(Util::VecScale(Line1 + Line2, 0.5),LineEnd, 2.f, sf::Color::Yellow, 0.f, sf::Color::Black);
			pRender->Draw(sN1);
			break;
		case 1:
			sL2 = sf::Shape::Line(Line1, Line2, 1.f, sf::Color::Red, 0.f, sf::Color::Red);
			pRender->Draw(sL2);
			sP2 = sf::Shape::Rectangle(TestPoint.x, TestPoint.y, 4, 4, sf::Color::Green, 1.f, sf::Color::Green);  
			pRender->Draw(sP2);
			sN1 = sf::Shape::Line(Util::VecScale(Line1 + Line2, 0.5),LineEnd, 2.f, sf::Color::Yellow, 0.f, sf::Color::Black);
			pRender->Draw(sN1);
			break;
		case 2:
			sL3 = sf::Shape::Line(Line1, Line2, 1.f, sf::Color::Red, 0.f, sf::Color::Red);
			pRender->Draw(sL3);
			sP3 = sf::Shape::Rectangle(TestPoint.x, TestPoint.y, 4, 4, sf::Color::Green, 1.f, sf::Color::Green);  
			pRender->Draw(sP3);
			sN1 = sf::Shape::Line(Util::VecScale(Line1 + Line2, 0.5),LineEnd, 2.f, sf::Color::Yellow, 0.f, sf::Color::Black);
			pRender->Draw(sN1);
			break;
		case 3:
			sL4 = sf::Shape::Line(Line1, Line2, 1.f, sf::Color::Red, 0.f, sf::Color::Red);
			pRender->Draw(sL4);
			sP4 = sf::Shape::Rectangle(TestPoint.x, TestPoint.y, 4, 4, sf::Color::Green, 1.f, sf::Color::Green);  
			pRender->Draw(sP4);
			sN1 = sf::Shape::Line(Util::VecScale(Line1 + Line2, 0.5),LineEnd, 2.f, sf::Color::Yellow, 0.f, sf::Color::Black);
			pRender->Draw(sN1);
			break;
		}
	}
#endif
}

void PhysicsObject::Register()
{
	//Do nothing
}